/* 
 * File:   robutils.hpp
 * Author: posypkin
 *
 * Auxilary utilities for working with robotic manipulator
 *
 * Created on July 8, 2012, 9:04 PM
 */

#ifndef ROBUTILS_HPP
#define	ROBUTILS_HPP

#include <string>
#include <vector>
#include <sstream>
#include <util/box/box.hpp>
#include <libjson/libjson.h>
#include "robot.hpp"
#include "robotdata.hpp"
#include "jsonnames.hpp"

template <class T> class RobUtils {
public:

    /**
     * Output description of RobotData to string 
     * @param rd robot data
     * @param str target string
     */
    static void toString(const RobotData<T>& rd, std::string& s) {
        std::ostringstream os;
        os << "dim: " << rd.mDim << "\n";
        os << "nsec: " << rd.mNSec << "\n";
        os << "eps: " << rd.mEps << "\n";
        os << "delta: " << rd.mDelta << "\n";
        os << "sections: [";
        for (int i = 0; i < rd.mNSec; i++) {
            os << rd.mSections[i];
            os << ",";
        }
        os << "]\n";
        os << "box: [";
        for(int i = 0; i < rd.mDim; i ++) {
            os << "(";
            os << rd.mBox->mA[i];
            os << ",";
            os << rd.mBox->mB[i];
            os << ")";
        }
        os << "]\n";
        if (rd.mAngles != NULL) {
            os << "angles: [";
            for (int i = 0; i < (rd.mDim - 1); i++) {
                os << rd.mAngles[2 * i];
                os << " : ";
                os << rd.mAngles[2 * i + 1];
                os << ",";
            }
            os << "]\n";
        }
        s = os.str();
    }

    /**
     * Parses Robotic data from JSON description
     * @param json
     * @param rd
     */
    static void fromJson(const std::string& json, RobotData < T >& rd) {

        struct {
            bool mDimSet;
            bool mNSecSet;
            bool mEpsSet;
            bool mDeltaSet;
            bool mSectionsSet;
            bool mBoxSet;
            bool mAnglesSet;
        } checklist = {false, false, false, false, false, false, false};

        JSONNode nd = libjson::parse(json);
        JSONNode::const_iterator i = nd.begin();

        while (i != nd.end()) {
            std::string name = i->name();
            std::cout << "node \"" << name << "\"\n";
            if (name == JSON_DIM) {
                rd.mDim = i->as_int();
                checklist.mDimSet = true;
            } else if (name == JSON_NSEC) {
                rd.mNSec = i->as_int();
                checklist.mNSecSet = true;
            } else if (name == JSON_EPS) {
                rd.mEps = i->as_float();
                checklist.mEpsSet = true;
            } else if (name == JSON_DELTA) {
                rd.mDelta = i->as_float();
                checklist.mDeltaSet = true;
            } else if (name == JSON_SECTIONS) {
                BNB_ASSERT(checklist.mNSecSet);
                rd.mSections = new T[rd.mNSec];
                BNB_ASSERT(rd.mSections);
                parseSections(*i, rd);
                checklist.mSectionsSet = true;
            } else if (name == JSON_BOX) {
                BNB_ASSERT(checklist.mNSecSet);
                rd.mBox = new Box<T> (rd.mNSec);
                parseBox(*i, rd);
                checklist.mBoxSet = true;
            } else if (name == JSON_ANGLE) {
                BNB_ASSERT(checklist.mDimSet);
                rd.mAngles = new T[2 * (rd.mDim - 1)];
                BNB_ASSERT(rd.mAngles);
                parseAngles(*i, rd);
                checklist.mAnglesSet = true;
            }
            i++;
        }
        BNB_ASSERT(checklist.mBoxSet);
        BNB_ASSERT(checklist.mSectionsSet);
        BNB_ASSERT(checklist.mNSecSet);
        BNB_ASSERT(checklist.mDimSet);
        BNB_ASSERT(checklist.mEpsSet);
        if (checklist.mAnglesSet)
            BNB_ASSERT(checklist.mDeltaSet);
    }

private:

    static void parseSections(const JSONNode& nd, RobotData < T > & rd) {
        JSONNode::const_iterator i = nd.begin();
        int j = 0;
        while (i != nd.end()) {
            BNB_ASSERT(j < rd.mNSec);
            rd.mSections[j] = i->as_float();
            j++;
            i++;
        }
    }

    static void parseAngles(const JSONNode& nd, RobotData < T > & rd) {
        JSONNode::const_iterator i = nd.begin();
        int j = 0;
        while (i != nd.end()) {
            BNB_ASSERT(j < (rd.mDim - 1) * 2);
            rd.mAngles[j] = i->as_float();
            j++;
            i++;
            rd.mAngles[j] = i->as_float();
            j++;
            i++;
        }
    }

    static void parseBox(const JSONNode& nd, RobotData < T > & rd) {
        JSONNode::const_iterator i = nd.begin();
        int j = 0;
        while (i != nd.end()) {
            BNB_ASSERT(j < rd.mNSec);
            rd.mBox->mA[j] = i->as_float();
            i++;
            rd.mBox->mB[j] = i->as_float();
            i++;
            j++;
        }
    }

};


#endif	/* ROBUTILS_HPP */

